#ifndef LOITORDRIVER_H
#define LOITORDRIVER_H

#include "ros/ros.h"
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include "sensor_msgs/Imu.h"
#include <cv_bridge/cv_bridge.h>

#include <pthread.h>
#include <cv.h>

#include "loitorusbcam.h"
#include "loitorimu.h"

#include <sstream>
#include <iostream>
using namespace std;
namespace loitor {
enum CAMERA {LEFT=0,RIGHT};

class intergrate_img
{
public:
  intergrate_img(){}
  intergrate_img(ros::NodeHandle n);
  void set_img(cv::Mat img);
  void set_stamp(timeval stamp);
  int load_cinfo(string info_url);
  void wrap_msgs();
  void new_image(bool resolution);
  void img_pub(string C);
private:
  //用于构造cv::Mat 的左右眼图像
  cv::Mat img;
  //当前左右图像的时间戳
  timeval time_stamp;
  // publish 到这两个 topic
  boost::shared_ptr<image_transport::ImageTransport> it;
  image_transport::CameraPublisher pub;
  sensor_msgs::ImagePtr msg;
  /***load calibration parameter***/
  boost::shared_ptr<camera_info_manager::CameraInfoManager> Info_manager;
  sensor_msgs::CameraInfoPtr info_ptr;// the calibration data
};

class loitordriver
{
public:
  loitordriver(){}
  loitordriver(ros::NodeHandle nh);
  ~loitordriver();
  void fill_image(int selection);
private:
  ros::NodeHandle n;
  ros::Publisher pub_imu;
  pthread_t imu_data_thread;
  // IMU data followed img reading
  visensor_imudata paired_imu;
  //imu viewer
  bool visensor_Close_IMU_viewer=false;
  bool imu_start_transfer=false;

  int static_ct=0;    ////???
  /**cmaera calibration infomation**/
  bool calibration_matches_;

  CAMERA camera;
  vector<shared_ptr<intergrate_img>> img;
private:
  void run();
  static void* imu_data_stream(void *arg);
  void Pub_leftimage();
  void Pub_rightimage();
  void Pub_stereoimage();
};

}
#endif // LOITORDRIVER_H
